#pragma once
#include "rclcpp/rclcpp.hpp"
#include <sensor_msgs/msg/joint_state.hpp>
#include <std_msgs/msg/float64.hpp>
#include <agv_msgs/msg/drivers_info.hpp>
#include <agv_msgs/msg/periph_status.hpp>
#include <agv_srvs/srv/outputs_new.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <vector>
#include <thread>
#include <mutex>
#include <agv_peripheral_controller/toolKit.h>
#define ROLLER_JOINT "roller_joint"
// #define AGV_PERIPHERAL_CONTROLLER_NODE_ID   2
///////////////////////////辊筒电机控制////////////////////////////////////////////
namespace peripheral_roller
{

	enum class MonitorState
	{
		Normal,
		JointTimeOut,
		DriverTimeOut
	};
	class CMotorControl
	{
	public:
		// 构造函数
		CMotorControl();

		// 析构函数
		~CMotorControl();

		// 初始化函数
		bool init();

		// 试图下使能-目前接口不能指定下指定电机的使能?
		bool tryHaltMotor();

		// 设置旋转速度
		bool rotate(float vel);

		// 电机是否正常
		bool fault();

		// 报错的具体信息
		std::string faultMsg();

		// 试图清除电机错误
		bool clearFault();

		// 停止旋转
		bool stop();

		// 实际辊筒运行状态
		bool rotateState();

		// 电机状态收集
		std::vector<agv_msgs::msg::PeriphMotorInfo> motorInfoCollect();

	private:
		// 关机状态回调函数
		void jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr &ms);

		// 驱动信息回调函数
		void driversInfoCallback(const agv_msgs::msg::DriversInfo::ConstSharedPtr &msg);

		// 静态线程函数
		static void thread_fun(CMotorControl *pThis);

		// 线程函数
		void thread_cycle();

		// 检测joint状态
		void checkJointState();

		// 设定目标速度
		void setVel(float radPerSecond);

		// 电机是否报错
		bool motorError();

		// 电机是否使能状态
		bool motorEnable();

		// 电机是否就绪状态
		bool motorReady();

	private:
		// 初始化完成标识
		bool m_init;

		// 控制速度-周期性输出
		std::atomic<float> m_fVel;

		// 辊筒电机半径
		float m_radius;

		// 关节状态-检测是否超时错误
		MonitorState m_state;

		// 带有时间戳的关节消息
		CDataTimeStamp<sensor_msgs::msg::JointState> m_jointState;

		// 带有时间戳的驱动消息
		CDataTimeStamp<agv_msgs::msg::DriversInfo> m_driverInfo;

		rclcpp::Node m_privNh;

		// 外设电机信息
		agv_msgs::msg::PeriphMotorInfo m_periphMotorInfo;

		// 状态订阅
		ros::Subscriber m_driverInfoSub;

		ros::Subscriber m_jointStateSub; /**< joint state from hardware interface */

		ros::Publisher m_rollerJointPub;

		// 线程相关
		std::thread m_thread;

		std::timed_mutex m_quit_mutex;

		// 挂起或复位客户端
		ros::ServiceClient m_clientHalt; /**< joint point halt contrl */

		ros::ServiceClient m_clientRecover; /**< joint point recover contrl */
	};
} // agv_peripheral